#include <pcl/io/openni_grabber.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>

int convertVFH (int x, float * ret) {
	  //Open file from argv[1]
	  pcl::PCDReader reader;
      pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudx (new pcl::PointCloud<pcl::PointXYZRGBA>);
	  std::stringstream ss;
      ss << "cloud_cluster_" << x << ".pcd";
      reader.read (ss.str(), *cloudx);

	  //find bounding box of cloud
	  Eigen::Vector4f min;
      Eigen::Vector4f max;
	  Eigen::Vector4f centr;
      pcl::getMinMax3D(*cloudx, min, max);
	  pcl::compute3DCentroid(*cloudx, centr);

	  ret[0]=min[0];
	  ret[1]=max[0];
	  ret[2]=min[1];
	  ret[3]=max[1];
	  ret[4]=min[2];
	  ret[5]=max[2];
	  ret[6]=centr[0];
	  ret[7]=centr[1];
	  ret[8]=centr[2];

      pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());
      // Create the VFH estimation class, and pass the input dataset+normals to it
      pcl::VFHEstimation<pcl::PointXYZRGBA, pcl::Normal, pcl::VFHSignature308> vfh;
      vfh.setInputCloud (cloudx);
      //normals
      // Create the normal estimation class, and pass the input dataset to it
	  pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
      ne.setInputCloud (cloudx);

      // Create an empty kdtree representation, and pass it to the normal estimation object.
      // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
      pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree0 (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
      ne.setSearchMethod (tree0);

      // Output datasets
      pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

      // Use all neighbors in a sphere of radius 3cm
      ne.setRadiusSearch (0.03);

      // Compute the features
      ne.compute (*cloud_normals);
      vfh.setInputNormals (cloud_normals);
      // alternatively, if cloud is of type PointNormal, do vfh.setInputNormals (cloud);

      // Create an empty kdtree representation, and pass it to the FPFH estimation object.
      // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
      pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
      vfh.setSearchMethod (tree);

      // Output datasets
      pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs (new pcl::PointCloud<pcl::VFHSignature308> ());

      // Compute the features
      vfh.compute (*vfhs);

      // vfhs->points.size () should be of size 1*
       pcl::io::savePCDFileASCII ("vfh_model.pcd", *vfhs);

	  return 0;
 }